#include <PID.h>

extern uint pwm_value;
extern float pid_pwm_value_num,speed;

struct{
				float shedingzhi;
				float shijizhi;
				float error;
				float error_last;
				float kp,ki,kd;
				float voltage;
				float integral;
				
}pid;


void pid_init()
{
	pid.shedingzhi=0.0;
	pid.shijizhi=0.0;
	pid.error=0.0;
	pid.error_last=0.0;
	pid.kp=5;
	pid.ki=0.15;
	pid.kd=0.2;
}


float pid_value(float speed)
{
	pid.shedingzhi=speed;
	
	pid.error=pid.shedingzhi-pid.shijizhi;
	
	pid.integral+=pid.error;
	
	pid.voltage=pid.kp*pid.error+pid.ki*pid.integral+pid.kd*(pid.error-pid.error_last);
	
	pid.error_last=pid.error;
	
	pid.shijizhi=pid.voltage*1.0;
	
	return pid.shijizhi;
}

//void up()
//{
//	left_vcc=1;
//	left_gnd=0;
//	rigth_vcc=1;
//	rigth_gnd=0;
//}


//void low()
//{
//	left_vcc=0;
//	left_gnd=1;
//	rigth_vcc=0;
//	rigth_gnd=1;
//}


//void pid_pwm_value()
//{
//	
//	pid.shijizhi=main_chaog();
//	pid_pwm_value_num=pid_value(speed);
//	if(pid_pwm_value_num<=speed)
//	{
//		up();
//	}else if(pid_pwm_value_num>speed)
//	{
//		low();
//	}
//	
//}



//void pid_pwm_value()
//{
//	
//	pid.shijizhi=main_chaog();
//	pid_pwm_value_num=pid_value(speed);
//	if(pid_pwm_value_num<=20)
//	{
////		pwm_value=1000-pid_pwm_value_num*50;
//		up();
//	}else if(pid_pwm_value_num>20)
//	{
//		low();
//		pid_pwm_value_num=pid_pwm_value_num-20.0;
////		pwm_value=pid_pwm_value_num*50-1000;
//	}
//	
//}
